InterfaceBoard* IB;
DCMotor* Mot;
Servo* Ser;
bool foundLight = false;
int flash_count = 0;
int freq_stop = 1;
int* buffer = (int*)calloc(2100, sizeof(int));

#define turn90 1500
#define FORWARD 90


void move(int leftSpeed, int rightSpeed)
{
    // Set speeds
    Mot->SetSpeed(0,-leftSpeed,100);
    Mot->SetSpeed(1,rightSpeed,100);
    Mot->SetSpeed(0,-leftSpeed,100);
    Mot->SetSpeed(1,rightSpeed,100);
}


int wait(unsigned long milisec)
{
    struct timespec req={0};
    time_t sec=(int)(milisec/1000);
    milisec=milisec-(sec*1000);
    req.tv_sec=sec;
    req.tv_nsec=milisec*1000000L;
    while(nanosleep(&req,&req)==-1)
         continue;
    return 1;
}

int randInt(int from, int to) 
{
	return from + (rand() % (to - from));
}

void test()
{
	move(-100,100);
	wait(10000);
	move(0,0);
	move(100,-100);
	wait(10000);
	move(0,0);
	wait(10000);
	move(100,100);
	wait(5000);
	move(0,0);
	wait(5000);
	move(-100,-100);
	wait(5000);
	move(0,0);	
}

void irScan()
{
	int ir = IB->GetSensorValue(6);
	int leftEdge, rightEdge;
		
	move(-100,-100);
	wait(500);
	move(0,0);
	wait(100);
	while (((ir - IB->GetSensorValue(6)) < 100 and IB->GetSensorValue(6) > 80) or IB->GetSensorValue(6) > 200)
	{	

		ir = IB->GetSensorValue(6);
		printf("IR raw: %i\n", ir);
		printf("IR cm: %i\n",2076/(ir - 11));
		move(-100,100);
		wait(50);
	}
	leftEdge = ir;
	move(0,0);
	printf("Left edge found\n");

	move(100,-100);
	wait(500);
	move(0,0);
	//wait(500);
	
	int c = 0;
	while (((ir - IB->GetSensorValue(6)) < 100 and IB->GetSensorValue(6) > 80) or IB->GetSensorValue(6) > 200)
	{
		ir = IB->GetSensorValue(6);
		printf("IR1: %i\n", ir);
		printf("IR2: %i\n",2076/(ir - 11));
		c++;
		move(100,-100);
		wait(50);
	}
	rightEdge = ir;
	printf("Right edge found\n");
	move(0,0);

	// convert values into cm
	leftEdge = 2076/(leftEdge - 11);
	rightEdge = 2076/(rightEdge - 11);
	printf("L: %i, R: %i,  L-R:%i\n",leftEdge, rightEdge, leftEdge-rightEdge);
	
	//if leftEdge > rightEdge, then we're closer to the right wall -> move backwards
	if (abs(leftEdge-rightEdge) > 5)
	{
		if(leftEdge > rightEdge)
		{
			move(-100,-100);
			wait(30*abs(leftEdge-rightEdge));
			move(0,0);		
			printf("LE > RE: %d\n", abs(leftEdge-rightEdge));
		} 
		else
		{
			move(100,100);
			wait(30*abs(leftEdge-rightEdge));
			move(0,0);
			printf("LE < RE: %d\n", abs(leftEdge-rightEdge));
		}
	}

	// turn 50*(c/2)to face switch [90 degrees = 1500ms]
	int turntime = 25*c + 500 + 200; //timeturned CW/2 + 500 + arbritraryAmount
	move(-100,100);wait(turntime);move(0,0);
	printf("Bisect turn time: %i\n",turntime);
	printf("Aligned\n");
}

void sonarScan()
{
	move(0,0);
	wait(500);
	int sonar = (int)(IB->GetSensorValue(7)*1.296);		
	
	while(sonar < 100 || sonar > 400){
		move(100,-100);
		wait(1);
		sonar = (int)(IB->GetSensorValue(7)*1.296);		
		//printf("sonar: %i cm\n", sonar);
	}
	// Found a gap - drive towards it and refind a RC
	move(100,100);
	wait(3000);
	move(0,0);
	while(sonar > 200){
		move(100,-100);
		wait(1);
		sonar = (int)(IB->GetSensorValue(7)*1.296);		
		//printf("sonar: %i cm\n", sonar);
	}
	move(0,0);
}




void lookForBlack()
{
	int lightValue = IB->GetSensorValue(4);
	int oldLightValue = lightValue;
	move(100,100);
	int randTime;
	int randDir;
	// checking not over black area
	while ((IB->GetSensorValue(4) - oldLightValue) < 5)
	{ 
		oldLightValue = IB->GetSensorValue(4);
		printf("Light Value: %i\n", oldLightValue); 
		
		wait(50);
		if(IB->GetInputState(5) == 1 or IB->GetInputState(6) == 1)
		{
			randTime = randInt(500,2000);
			if(IB->GetInputState(5) == 1 and IB->GetInputState(6) == 0)
			{
				move(-100,-100);
				wait(300);
				move(0,0);
				move(100,-100);
				wait(randTime);
				move(100,100);
			}
			else if (IB->GetInputState(6) == 1 and IB->GetInputState(5) == 0) 
			{
				move(-100,-100);
				wait(300);
				move(0,0);
				move(-100,100);
				wait(randTime);
				move(100,100);
			}
			else
			{
				randDir = randInt(0,100);
				move(-100,-100);
				wait(300);
				move(0,0);
				if(randDir < 50)
				{
					move(100,-100);
					wait(randTime);
					move(100,100);
				}
				else
				{
					move(-100,100);
					wait(randTime);
					move(100,100);
				}
			}
		}
		
	}
	move(0,0);
	printf("BLACK\n");
}
/*
void lookForBlack(int c, bool seenBlack)
{
	int lightValue = IB->GetSensorValue(4);
	int oldLightValue = lightValue;
	move(100,100);
	int sb = 0;
	if(seenBlack) sb = 1;
	printf("C %i  SeenBlack %i \n ",c,sb);
	while ((IB->GetSensorValue(4) - oldLightValue) < 5)
	{ 
		oldLightValue = IB->GetSensorValue(4);
		printf("Ground light value: %i\n", oldLightValue); 
		wait(50);
		if(IB->GetInputState(5) == 1 or IB->GetInputState(6) == 1)
		{
			goRandom();
			break;
		}
	}
	move(0,0);
	printf("BLACK\n");
}
*/
void lookForSwitch()
{
	printf("Going towards the light switch....\n");
	for(int i = 0; i < 10; i++)
	{
		move(100,100);
	}

	while (IB->GetInputState(5) == 0 and IB->GetInputState(6) == 0){wait(1);}
	move(0,0);
}

void lookForLight()
{

	int pos = 0;
	foundLight = false;
	int cLight = IB->GetSensorValue(3);
	
	// #### If hit head on then may detect light straight away ####
		cLight = IB->GetSensorValue(3);
	int waited = 0;
	while(cLight <= 100 && waited < 1000)
	{	
		cLight = IB->GetSensorValue(3);
		wait(1);
		waited++;
	}
	
	if(cLight > 100) 
	{
		printf("  - light2: %i\n",cLight);
		pos = Ser->GetPosition();
		foundLight = true;
		printf("LIGHT!\n");
	}
	
	if(!foundLight){
		Ser->SetPosition(0);
		wait(500);
		for(int i = 0; i < 160; i+=10)
		{
			cLight = IB->GetSensorValue(3);
			printf("i:%i, light:%i, pos:%i\n",i,cLight, Ser->GetPosition());
			Ser->SetPosition(i);
			//wait(1000);
			cLight = IB->GetSensorValue(3);
			int waited = 0;
			while(cLight <= 100 && waited < 1000)
			{	
				cLight = IB->GetSensorValue(3);
				wait(1);
				waited++;
			}

			if(cLight > 100) 
			{
				printf("  - light2: %i\n",cLight);
				pos = Ser->GetPosition();
				foundLight = true;
				printf("LIGHT!\n");
				break;
			}
		}
	}
	
	/*
	if(pos != 0)
	{
		Ser->SetPosition(pos);
	  	wait(800);
		foundLight = true;
		printf("LIGHT!\n");
	}
	else {printf("NO LIGHT!\n");}
	*/
}

void checkFreq()
{
	int newLight = IB->GetSensorValue(3);
	//int oldLight = newLight;
	printf("Checking frequency...\n");
	

	for(int i = 0; i < 2000; i ++)
	{
		wait(1);	
		newLight = IB->GetSensorValue(3);
		if(newLight > 100)//100
		{ 
			//oldLight = newLight; 
			buffer[i] = 1;
		}
		else {
			//oldLight = newLight;
			buffer[i]=0;
		}
		if (i >= 2100) {i = 2099;}
		printf("newLight = %d\n", newLight);
	}
}

float analyzeFreq()
{
	int currentValue = buffer[0];
	flash_count = 0;
	for(int i = 1; i < 2000; i++)
	{
		if(currentValue == 0 and buffer[i] == 1) flash_count++;
		currentValue = buffer[i];
	}
	if (flash_count == 0) return -1.0;
	else return floor((float)flash_count/2.0);
}

void goBack()
{
	printf("Going back...\n");
	int oldLightValue = IB->GetSensorValue(4);
	move(-100,-100);
	// int sb = 0;
	// if(seenBlack) sb = 1;
	// printf("C %i  SeenBlack %i \n ",c,sb);
	while (abs(IB->GetSensorValue(4) - oldLightValue) < 5){oldLightValue = IB->GetSensorValue(4); wait(50);}
	move(0,0);
}

int idRS(float freq)
{
	int site;
    //Internal RC identification table
    /*
	Site	Hz	Picture
	1	0.5	Terminator
	2	1
	3	2
	4	4
	5	6	Short Circuit
	6	8	Walle
    */ 
	// 0.5Hz
	if(freq == 0) site = 6;
	// 1Hz
	else if(freq < 2 and freq > 0) site = 2;
	// 2Hz
	else if(freq < 4 and freq >= 2) site = 4;
	// 4Hz
	else if(freq < 6 and freq >= 4) site = 5;
	// 6Hz
	else if(freq < 8 and freq >= 6) site = 3;
	// 8Hz
	else if(freq >= 8) site = 1;
	
	// move back, turn ACW 360
	if (site == 6)
	{
		move(-100,-100);
		wait(1000);
		move(-100,100);
		wait(turn90*4);
		move(0,0);
		wait(1000);
		
		// Reverse movement
		move(100,-100);
		wait(turn90*5);
		move(0,0);
	}
	// move back, turn CW 360	
	if (site == 2)
	{
		move(-100,-100);
		wait(1000);
		move(0,0);
		move(100,-100);
		wait(turn90*4);
		move(0,0);
		wait(1000);
		

		// Reverse movement
		move(-100,100);
		wait(turn90*5);
		move(0,0);
	}
	// move back, stop, move back
	if (site == 4)
	{
		move(-100,-100);
		wait(1000);
		move(0,0);
		wait(500);
		move(-100,-100);
		wait(1000);
		move(0,0);
		wait(1000);
		
		// Reverse movement
		move(100,100);
		wait(500);
		move(0,0);
	}
	// move back, turn ACW 180
	if (site == 5)
	{	
		move(-100,-100);
		wait(1000);
		move(0,0);
		move(-100,100);
		wait(turn90*2);
		move(0,0);
		wait(1000);

		// Reverse movement
		move(100,-100);
		wait((turn90*2) + 700);
		move(0,0);
		
	}
	// move back, turn CW 180		
	if (site == 3)
	{
		move(-100,-100);
		wait(1000);
		move(0,0);
		move(100,-100);
		wait(turn90*2);
		move(0,0);
		wait(1000);

		// Reverse movement
		move(-100,100);
		wait((turn90*2) + 700);
		move(0,0);
	}
	// move back, move back, stop, move forward
	if (site == 1)
	{
		move(-100,-100);
		wait(2000);
		move(0,0);
		wait(500);
		move(100,100);
		wait(1000);
		move(0,0);
		wait(1000);

	}

    printf("RC ID: %i\n", site);
    return site;
}

void exitRS(int rc)
{
	int ir;
	move(100,100);
	wait(500);
	move(-100,-100);
	wait(100);
	move(0,0);
	bool bumped = false;
	if(rc == 1)
	{
		while (IB->GetSensorValue(6) > 200)
		{
			// turn left until see nothing in front
			ir = IB->GetSensorValue(6);
			move(-100,100);
			wait(100);
		}
		// turn a little bit more
		wait(400);
		move(0,0);
		Ser->SetPosition(FORWARD + 70);
		wait(700);	
		
		while (IB->GetSensorValue(6) > 200)
		{
			move(100,100); 
			wait(100);
			if(IB->GetSensorValue(6) > 200)
			{
				ir = IB->GetSensorValue(6);
				//printf("%i\n",ir);
			}
			// if we hit the right bumper
			// turn away from the wall a little bit
			if(IB->GetInputState(6) == 1)
			{
				move(0,0);
				move(100,-100);
				wait(500);
				move(100,100);
			}
		}
		// end of wall follow - clear the wall
		wait(400);
		// turn right a bit
		move(100,-100);
		wait(300);
		// go towards rs 2
		move(100,100);
		wait(300);
		move(0,0);
	}
	if(rc == 2)
	{
		// turn left until nothing in front
		while (IB->GetSensorValue(6) > 200)
		{
			ir = IB->GetSensorValue(6);
			move(-100,100);
			wait(100);
		}
		// turn a little bit more
		wait(200);
		move(0,0);
		Ser->SetPosition(FORWARD+70);
		wait(700);	

		while (IB->GetSensorValue(6) > 200)
		{
			move(100,100); 
			wait(100);
			if(IB->GetSensorValue(6) > 200)
			{
				ir = IB->GetSensorValue(5);
				//printf("%i\n",ir);
			}
			// if we hit the right bumper
			// turn away from the wall a little bit
			if(IB->GetInputState(6) == 1)
			{
				move(0,0);
				move(-100,100);
				wait(500);
				move(100,100);
				bumped = true;
			}
		}
		// adjust heading if bumped the wall
		move(0,0);
		
	}
	if(rc == 3)
	{
		move(-100,-100);
		wait(400);
		// turn right until nothing in front
		while (IB->GetSensorValue(6) > 200)
		{
			ir = IB->GetSensorValue(6);
			move(100,-100);
			wait(100);
		}
		// move a little bit more
		wait(200);
		move(0,0);
		
		// turn servo to look at left wall
		Ser->SetPosition(0);
		wait(500);
		
		while (IB->GetSensorValue(6) > 200)
		{
			move(100,100); 
			wait(100);
			if(IB->GetSensorValue(6) > 200)
			{
				ir = IB->GetSensorValue(6);
				//printf("%i\n",ir);
			}
			// if we hit the left bumper
			// turn away from the wall a little bit
			if(IB->GetInputState(5) == 1)
			{
				move(0,0);
				move(-100,100);
				wait(200);
				move(100,100);
				bumped = true;
			}
		}
		// drive forward
		move(100,100);
		move(0,0);
	}
	if(rc == 4)
	{
		// turn right until nothing in front
		while (IB->GetSensorValue(6) > 200)
		{
			ir = IB->GetSensorValue(6);
			move(100,-100);
			wait(100);
		}
		// turn a little bit more
		wait(200);
		move(0,0);
		Ser->SetPosition(FORWARD);
		wait(1000);
		move(100,100);
		// drive forward for 4 seconds if hit left bumper - adjust
		for (int t=0; t<40; t++){
			wait(100);
			if(IB->GetInputState(5) == 1){
						move(0,0);
						move(100,-100);
						wait(500);
						move(100,100);
						bumped = true;
			}
		}
	
		move(100,-100);
		wait(turn90);
		move(0,0);
		
	}
	if(rc == 5)
	{

		// turn right until nothing in front
		while (IB->GetSensorValue(6) > 200)
		{
			ir = IB->GetSensorValue(6);
			move(100,-100);
			wait(100);
		}
		// turn a little bit more
		wait(200);
		move(0,0);

		Ser->SetPosition(0);
		wait(500);	
		while (IB->GetSensorValue(6) > 200)
		{
			move(100,100); 
			wait(100);
			if(IB->GetSensorValue(6) > 200)
			{
				ir = IB->GetSensorValue(5);
				//printf("%i\n",ir);
			}
			// if we hit the left bumper
			// turn away from the wall a little bit
			if(IB->GetInputState(5) == 1)
			{
				move(0,0);
				move(100,-100);
				wait(500);
				move(100,100);
				bumped = true;
			}
		}
		// drive past the end of the wall
		wait(1000);
		move(0,0);
		move(-100,100);
		wait(turn90 + 700);
		move(100,100);
		wait(3000);
		move(0,0);	 
	}
	move(0,0);
}

